#ifndef MAPTRANSFORMER_H
#define MAPTRANSFORMER_H

#include <QObject>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <fstream>
#include <PathCut.h>
#include <MaximumSubRectDivision.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <boost/bind.hpp>
#include <DARPPlanner.h>
#include <Dinic.h>
#include <m_TSP.h>
#include <HeuristicPartition.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <QThread>


using std::cout;
using std::endl;
using std::ios;

#define PI 3.141592654
#define HORIZONTAL 0
#define VERTICAL   1
#define FIRST 0
#define LAST  1
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;


using namespace  std;

class MapTransformer : public QThread
{
    Q_OBJECT
public:
    explicit MapTransformer(QObject *parent = nullptr);


    void setToleranceDistance(double toleranceDistance);
    void setRobotNum(int robotNum);
    void setAllocateMethod(string allocateMethod);
    void setMstShape(string MSTShape);
    void setuseROSPlanner(bool useROSPlanner);
    void setMapScale(int mapScale);
    void setCoverageAction(bool coverageAction);
    void setGAMaxIter(int num);
    void setUnchangedIter(int num);
    void setHpMaxiter(int num);

    void setMap(const nav_msgs::OccupancyGrid& msg);


    bool createPaths(vector<geometry_msgs::PoseWithCovarianceStamped> &vecInitPose);
     bool createPaths();
     vector<nav_msgs::Path>& getPath(){return paths;}

protected:
    void run() override;

private:
    void showMapAndRegionInf();
    void eliminateIslands();
    void sendOneGoal(int id, int step, MoveBaseClient &ac);
    bool reachGoal(int id, int step);
    void tocc(unsigned char order);
    void checkActionState(MoveBaseClient &ac);
    Mat shortenPathsAndGetCheckpoints(Mat& paths_cpt_idx);
    bool isSameLine(int& a, int& b, int& c);
    void pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&  odom, int &id);
    void getPathInfo(Mat& path);
    void sendGoals();
    bool setInitPose(vector<geometry_msgs::PoseWithCovarianceStamped> &vecInitPose);



signals:
    void emitPath(vector<nav_msgs::Path>& vecPath);

private:
    nav_msgs::OccupancyGrid map;
    nav_msgs::OccupancyGrid coverage_map;
    ros::Publisher coverage_map_pub;
    bool received_map;
    int cmw, cmh;  // coverage map width, height and
    double cmres;  // coverage map resolution
    int map_scale = 10;

    // path plan
    vector<int> robot_init_pos;
    Mat Map, Region, MST, paths_idx;
    vector<nav_msgs::Path> paths;

    // goal control
    move_base_msgs::MoveBaseGoal goal;
    vector<pair<double, double> > robot_pos;
    Mat paths_cpt_idx;
    vector<int> goal_ptr;
    vector<int> cpt_ptr;

    std::vector<ros::Publisher> path_publishers;
    std::vector<ros::Subscriber> odom_subscribers;

    // timer
    double tic, toc;

    // ros param
    int robot_num;
    double tolerance_distance;
    string MST_shape;
    string allocate_method;
    string data_file_path;

    unsigned int pose_callback_cnt;
    vector<bool> initpos_flag;

    std::fstream test_data_file;
    bool useROSPlanner, coverAndReturn, coverage_action;


    // GA config
    int GA_max_iter, unchanged_iter;
    MTSP *mtsp;

    // heuristic
    int hp_max_iter;
};

#endif // MAPTRANSFORMER_H
